#pragma once
#include <Math/Matrix3x3.hpp>
#include <Math/Random.hpp>
#include <Math/Vector.hpp>

namespace zzz{
template<typename T, zuint N>
Vector<N+1,T> ToHomogeneous(const Vector<N,T> &v)
{
  Vector<N+1,T> res;
  for (zuint i=0; i<N; i++) res[i]=v[i];
  res[N]=1;
  return res;
}

template<typename T, zuint N>
Vector<N-1,T> FromHomogeneous(const Vector<N,T> &v)
{
  Vector<N-1,T> res;
  for (zuint i=0; i<N-1; i++) res[i]=v[i]/v[N-1];
  return res;
}

//randomly pick N different integers
template<zuint N>
class RANSACPicker
{
public:
  typedef Vector<N,int> PickType;
  /// Constructor
  RANSACPicker(const int min, const int max)
      : randi_(min,max) {
    ZCHECK_GE(max - min + 1, N) << "Cannot pick enough number from the given range!";
  }
  Vector<N,int> Pick() {
    Vector<N,int> res;
    set<int> taken;
    for (zuint i = 0; i < N; i++) {
      int x = randi_.Rand();
      while(taken.find(x) != taken.end()) x = randi_.Rand();
      taken.insert(x);
      res[i] = x;
    }
    return res;
  }
  void SeedFromTime(){randi_.SeedFromTime();}
private:
  RandomInteger<int> randi_;
};

template<typename T>
Matrix<3,3,T> SkewSymmetricMatrix(const Vector<3,T> &a)
{
  Matrix<3,3,T> res;
  res(0,0)=0;    res(0,1)=-a[2];   res(0,2)=a[1];
  res(1,0)=a[2]; res(1,1)=0;       res(1,2)=-a[0];
  res(2,0)=-a[1];res(2,1)=a[0];    res(2,2)=0;
  return res;
}
}

